#include "t1_crypto.h"
#include "iodev.h"
#include <string.h>
#include "clk.h"
#include "ioctl.h"
#include "config.h"

typedef struct {
        uint32_t total[2];
        uint8_t buffer[64];
        uint32_t times;
}sha256_context;

static sha256_context ctx;

static inline void sha256_copy(uint8_t *dst, uint8_t *src, int length)
{
        dst += length;
        while (length -- > 0) {
                *--dst = *src++;
        }
}
static void sha256_process(const uint8_t data[64])
{
        sha256_copy(SHA256_IN, (uint8_t *)data, SHA256_INLEN);
        if (ctx.times) {
                SHA256->CCFG = SHA256_EN | SHA256_WR1;
        }
        else {
                SHA256->CCFG = SHA256_EN | SHA256_WR0;
        }
        ctx.times ++;
        while (!(SHA256->INT_STA & SHA256_DONE));
        SHA256->INT_CLR = SHA256_DONE;
        return;
}

static void sha256_update(const uint8_t * input, uint32_t length)
{
        uint32_t left, fill;

        if (!length)
                return;

        left = ctx.total[0] & 0x3F;
        fill = SHA256_INLEN - left;

        ctx.total[0] += length;
        ctx.total[0] &= 0xFFFFFFFF;

        if (ctx.total[0] < length)
                ctx.total[1]++;

        if (left && length >= fill) {
                memcpy(ctx.buffer + left, input, fill);
                sha256_process(ctx.buffer);
                length -= fill;
                input += fill;
                left = 0;
        }

        while (length >= SHA256_INLEN) {
                sha256_process(input);
                length -= SHA256_INLEN;
                input += SHA256_INLEN;
        }

        if (length)
                memcpy(ctx.buffer + left, input, length);
}

static const uint8_t sha256_padding[64] = {
        0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
#define PUT_UINT32_BE(n,b,i) {                          \
        (b)[(i)    ] = (unsigned char) ( (n) >> 24 );   \
        (b)[(i) + 1] = (unsigned char) ( (n) >> 16 );   \
        (b)[(i) + 2] = (unsigned char) ( (n) >>  8 );   \
        (b)[(i) + 3] = (unsigned char) ( (n)       );   \
}
static void sha256_finish(uint8_t digest[32])
{
        uint32_t last, padn;
        uint32_t high, low;
        uint8_t msglen[8];

        high = ((ctx.total[0] >> 29)
                | (ctx.total[1] << 3));
        low = (ctx.total[0] << 3);

        PUT_UINT32_BE(high, msglen, 0);
        PUT_UINT32_BE(low, msglen, 4);

        last = ctx.total[0] & 0x3F;
        padn = (last < 56) ? (56 - last) : (120 - last);

        sha256_update(sha256_padding, padn);
        sha256_update(msglen, 8);

        SHA256->CCFG = SHA256_EN | SHA256_RD;
        while (!(SHA256->INT_STA & SHA256_DONE));
        SHA256->INT_CLR = SHA256_DONE;

        sha256_copy(digest, SHA256_OUT, SHA256_OUTLEN);
}

static int sha256_init(void)
{
        
        ctx.total[0] = ctx.total[1] = ctx.times = 0;
        SHA256->INT_EN |= SHA256_DONE;
        SHA256->INT_MSKED &= ~SHA256_DONE;
        
        return 0;
}

static ssize_t sha256_read(iodev_t *dev, void *buffer, size_t size)
{
        if (size > SHA256_OUTLEN)
		size = SHA256_OUTLEN;

        if (!is_enable_crypto_clk()) {
            __enable_crypto_clk();
        }
        sha256_finish(buffer);
        ctx.total[0] = ctx.total[1] = ctx.times = 0;
		memset( SHA256_OUT, 0x00, SHA256_OUTLEN);
        
        return size;
}

static ssize_t sha256_write(iodev_t *dev, const void* buffer, size_t size)
{
        if (!is_enable_crypto_clk()) {
            __enable_crypto_clk();
        }
        sha256_update(buffer, size);
        
        return size;
}

static int sha256_ioctl(iodev_t *dev, uint32_t cmd, void *arg)
{
        switch (cmd) {
        case IOCTL_CRYPTO_START:
                __enable_crypto_clk();
                sha256_init();
                break;
        case IOCTL_CRYPTO_STOP:
                __disable_crypto_clk();
                break;
        default:
                break;

        }
        return 0;
}

static const iodev_t T1_sha256 __iodev_entry = {
        .name = "sha256",
        .priv = NULL,
        .init = NULL,
        .read = sha256_read,
        .write = sha256_write,
        .ioctl = sha256_ioctl,
        .poll = NULL,
#ifdef CONFIG_POWER_CTL
        .suspend = NULL,
        .wakeup = NULL
#endif
};


